 //
// Created by Sonny on 2023/10/2.
//
#include "UPRE_Alg_PID.h"

#define ABS(x)		((x>0)? x: -x)



void PID::PID_SET(float kp, float ki, float kd,float kf){
    Kp = kp;
    Ki = ki;
    Kd = kd;
    Kf = kf;
}

void PID::PID_IMAX() {
    /*积分限幅*/
    if(I_OUT_MAX != 0.f){
        if (i_out[0] < -I_OUT_MAX)i_out[0] = -I_OUT_MAX;
        else if(i_out[0] > I_OUT_MAX)i_out[0] = I_OUT_MAX;
    }
}

void PID::PID_DeadZone() {
    /*死区*/
    if (ABS(Error[0]) < Dead_Zone) {
        Error[0] = 0;
        OUt[0] = 0;
    }
}

void PID::PID_OUTMAX() {
    /*输出限幅*/
    if(OUT_MAX != 0.0f){
        if (OUt[0] < -OUT_MAX)OUt[0] = -OUT_MAX;
        else if(OUt[0] > OUT_MAX)OUt[0] = OUT_MAX;
    }
}

void PID::PID_FeedForward() {
    /*前馈输出*/
    f_out = (Target[0] - Target[1]) * Kf;
    OUt[0] = p_out + i_out[0] + d_out + f_out;
}

void PID::PID_Calc() {


    Error[0] = Target[0] - Now[0];
    Delta_Error[0] = Error[0] - Error[1];
    Delta_Error[0] = Error[1] - Error[2];


    if(Mode == PID_Position){
        p_out = Kp * Error[0];
        d_out = Kd * (Error[0] - Error[1]);
        /*梯形积分(提高积分项的运算精度)*/
        i_out[0] += Ki * 0.5f * (Error[0]+Error[1]);

        PID_IMAX();


        if (Expert != 0){
            /*规则1:误差超过设定的最大误差判定值(H_E),进行开环强控制*/
            if (ABS(Error[0]) > High_Error){
                OUt[0] = OUT_MAX;
            }

            /*规则2:误差小于设定的最小误差判定值(L_E),Kp和Ki减小进行弱控制,当偏差到一定程度后引入死区*/
            if (ABS(Error[0]) <= Low_Error){
                if (ABS(Error[0]) > Dead_Zone) {
                    OUt[0] = 0.5f * p_out + 0.3f * i_out[0];
                } else OUt[0] = 0;
            }

            /*规则3:误差的绝对值在减小或者误差达到平衡,保持上一次的输出*/
            if ((Error[0] * Delta_Error[0] < 0 && Delta_Error[0] * Delta_Error[1] > 0) || Error[0] == 0){
                OUt[0] = OUt[1];
            }

            /*规则4:误差处于极值状态,若误差绝对值超过(M_E)实行较强控制,若误差绝对值小于(M_E)实行较弱控制*/
            if (Error[0] * Delta_Error[0] < 0 && Error[0] * Delta_Error[1] < 0) {
                if (ABS(Error[0]) > Mid_Error) OUt[0]= 1.8f * Error[0];
                if (ABS(Error[0]) > Mid_Error) OUt[0]= 1.2f * Error[0];
            }

            /*规则5:误差的绝对值在变大或者误差变化量维持常值,若误差绝对值超过(M_E)实行较强控制,若误差绝对值小于(M_E)实行一般较弱控制*/
            if (Error[0] * Delta_Error[0] > 0 || Delta_Error[0] == 0) {
                if (ABS(Error[0]) > Mid_Error) OUt[0]= 2.0f * (p_out + i_out[0] + d_out);
                if (ABS(Error[0]) < Mid_Error) OUt[0]= 1.1f * (p_out + i_out[0] + d_out);
            }
        } else{ OUt[0] = p_out + i_out[0] + d_out;}

//        PID_FeedForward();
        PID_OUTMAX();
        PID_DeadZone();
    }
    else if(Mode == PID_Delta){
        p_out = Kp * (Error[0]-Error[1]);
        d_out = Kd * (Error[0] - 2*Error[1] + Error[2]);
        /*梯形积分(提高积分项的运算精度)*/
        i_out[0] =Ki * 0.5f * (Error[0]+Error[1]);

        PID_DeadZone();
        if (Expert != 0) {
            /*规则1*/
            if (ABS(Error[0]) > High_Error) {
                OUt[0] = OUT_MAX;
            }

            /*规则2*/
            if (ABS(Error[0]) <= Low_Error) {
                if (ABS(Error[0]) > Dead_Zone) {
                    OUt[0] = OUt[1] + 0.5f * p_out + 0.5f * i_out[0];
                } else OUt[0] = 0;
            }

            /*规则3*/
            if ((Error[0] * Delta_Error[0] < 0 && Delta_Error[0] * Delta_Error[0] > 0) || Error[0] == 0) {
                OUt[0] = OUt[1];
            }

            /*规则4*/
            if (Error[0] * Delta_Error[0] < 0 && Error[0] * Delta_Error[1] < 0) {
                if (ABS(Error[0]) > Mid_Error) OUt[0] = OUt[1] + 1.5f * p_out;
                if (ABS(Error[0]) < Mid_Error) OUt[0] = OUt[1] + 0.6f * p_out;
            }

            /*规则5*/
            if (Error[0] * Delta_Error[0] > 0 || Delta_Error[0] == 0) {
                if (ABS(Error[0]) > Mid_Error) OUt[0] = OUt[1] + 2.0f * (p_out + i_out[0] + d_out);
                if (ABS(Error[0]) < Mid_Error) OUt[0] = OUt[1] + 0.8f * (p_out + i_out[0] + d_out);
            }
        }else OUt[0] = p_out + i_out[0] + d_out;

        PID_OUTMAX();
        PID_DeadZone();
    }

    Error[2] = Error[1];
    Error[1] = Error[0];
    i_out[1] =i_out[0];
    OUt[1] = OUt[0];
    Delta_Error[1] = Delta_Error[0];
}

void PID::PID_ModInit(uint8_t mode, uint8_t expert, float i_OUT_MAX, float oUT_MAX, float dead_Zone){

    Mode = mode;
    Mode = expert;
    I_OUT_MAX = i_OUT_MAX;
    OUT_MAX = oUT_MAX;
    Dead_Zone = dead_Zone;

}
